#! /usr/bin/env python
import sys
sys.path.append('/home/robot/catkin_ws/src')
import rospy
from planning.scripts.fingers_action_client import unitParser as finger_pos_unitParser
from planning.scripts.pose_action_client import unitParser as eef_pose_unitParser
from planning.scripts.robot_control_modules import *
from planning.scripts.action_client import *

if __name__ == '__main__':
    rospy.init_node('test_action_servers')
    robot_prefix = 'j2n6s300_'
    initial_joint_pose(robot_prefix)
    # arm_change_joint_action([0,0,0,0,0,-90,0], robot_prefix)
    # arm_change_joint_action([0,0,0,0,0,90,0], robot_prefix)
    # relative = False
    # prefix = 'j2n6s300_'
    # finger_positions = [10,50,80]
    # pose = [-0.05, -0.098, 0.5857, 0.87676, 0.085871, -0.01236, 0.47304]

    # finger_turn, finger_meter, finger_percent = \
    #     finger_pos_unitParser('percent', finger_positions, relative)
    # pose_mq, pose_mdeg, pose_mrad = eef_pose_unitParser('mq', pose, relative)
    
    # try:
    #     poses = [float(n) for n in pose_mq]
    #     result = cartesian_pose_client(poses[:3], poses[3:], prefix)
    #     result = gripper_client(finger_turn, prefix)
    # except rospy.ROSInterruptException:
    #     print('program interrupted before completion')